#include <joyos.h>
#include <happylib.h>
#include <math.h> 
#include "define.h"
#include "gyrocalibrate.h"

void gyroSetDegree(){
	if(fabs((float)gyro_get_degrees()-180)>90){
		gyro_set_degrees(0);
	}else{
		gyro_set_degrees(180);
	}
}

// !!!!!!! BUMPER_BACK_LEFT is a constant !!!!!!!!!!!
void hitWallLeftgyroSet(){
	if(BUMPER_BACK_LEFT==1 && BUMPER_BACK_RIGHT == 1 && BUMPER_WALL_LEFT==1){
		gyroSetDegree();
	}
}
void hitWallRightgyroSet(){ // Calibrate when right of robot hit wall
	if(BUMPER_BACK_LEFT==1 && BUMPER_BACK_RIGHT == 1 && BUMPER_WALL_RIGHT==1){
		gyroSetDegree();
	}
}

